%% Time Domain Filter
% Script by adq@XJTU, Jan. 1, 2022
% Requires movfun.m harmonics.m
% padding = 'repeat'; pAlign = 'right';
% A = movfun(@filterfcn, data, n_slide, n_overlap, padding, pAlign);
%  y = harmonics(frequencies, amplitudes, phases, timeVect)
% 时域滤波实验
% 时域滑窗中值
%% 测试数据 1
% 检验是否正确应用了算法
x = [1 2 3 4 5 6];
%% 测试数据 2
% 阶跃+斜坡的参考信号, 叠加白噪声和若干个频点的干扰信号
Fs = 200;
tspan = 0.8;
t = 0 : 1/Fs : tspan;
datalen = length(t);

step_size = 2;

ref = zeros(1,datalen);

bias = floor(length(ref) / 3); % step
ref(bias:end) = ref(bias:end) + step_size;

bias1 = floor(2*length(ref) / 3); % down step with ramp
ref(bias1:end) = ref(bias1:end) - ...
	step_size .* gredge(length(ref)-bias1+1, 'linear', (length(ref)-bias1+1) / 2); 

rng('default'); % white noise
x = ref + .1.*randn(1,datalen);

freq = [20 50 60 70];
amp = [.1 1 1 .2];
phases = [0 0 0 0];
x = x + harmonics(freq, amp, phases, t);
%%
nSlide = 9;
nAppend = 1;
nOverlap = nSlide - nAppend;

padding = 'repeat';
pAlign = 'right';

figure('ToolBar', 'none', 'MenuBar', 'figure');
% colormap lines
hold on;
plot(t, ref, 'k-', 'DisplayName', 'Reference');
plot(t, x, ':', 'DisplayName', 'Raw', 'LineWidth', .1);

% result = movfun(@fir_lowpass20Hz50Hz_FS200Hz, x, nSlide, nOverlap, padding, pAlign);
% y = NaN .* zeros(size(x)); % NaN to supress plot over invalid points
% y(end-length(result)+1:end) = result;
% plot(t, y, 'o-', 'DisplayName', 'FIR LPF');

% result = movfun(@fir_notch5060Hz_FS200Hz, x, nSlide, nOverlap, padding, pAlign);
% y = NaN .* zeros(size(x)); % NaN to supress plot over invalid points
% y(end-length(result)+1:end) = result;
% plot(t, y, 'o', 'DisplayName', 'FIR Notch');

% result = movfun(@mean_noKpeeks, x, nSlide, nOverlap, padding, pAlign);
% y = NaN .* zeros(size(x)); % NaN to supress plot over invalid points
% y(end-length(result)+1:end) = result;
% plot(t, y, '+-.', 'DisplayName', 'Adv Mean');

% result = movfun(@mean, x, nSlide, nOverlap, padding, pAlign);
% y = NaN .* zeros(size(x)); % NaN to supress plot over invalid points
% y(end-length(result)+1:end) = result;
% plot(t, y, '-', 'DisplayName', 'Mean');

legend('Location', 'northwest', 'FontSize', 15);
xlabel('t s');
hold off;

function y = fir_notch5060Hz_FS200Hz(x) % len = 9
	if iscolumn(x)
		x = x';
	end
	y = x * [-0.0706, -0.0594, 0.20646, 0.12376, 0.55413, 0.12376, 0.20646, -0.0594, -0.0706]';
end

function y = fir_lowpass20Hz50Hz_FS200Hz(x) % len = 9
	if iscolumn(x)
		x = x';
	end
	y = x * [-0.05807, -0.02746, 0.106047, 0.291691, 0.380367, 0.291691, 0.106047, -0.02746, -0.05807]';
end

function y = mean_noKpeeks(x) % 舍弃滑窗中最大/小各 k 个值后取平均
	k = 2; % 0 = 简单平均, x 长为 9 取 4 为中值平均
	[~, maxidx] = maxk(x, k);
	[~, minidx] = maxk(-x, k);
	x(maxidx) = NaN;
	x(minidx) = NaN;
	y = mean(x, 'omitnan');
end
